/**
 ******************************************************************************
 * @文件    sbus_parser.c
 * @作者    SBUS解析器
 * @版本    V1.0
 * @日期    2024-01-01
 * @概要    SBUS协议解析器实现文件
 ******************************************************************************
 */

#include <stdio.h>
#include <string.h>
#include "sbus_parser.h"

// SBUS帧长度
#define SBUS_FRAME_LENGTH 25
#define FALSE 0
#define TRUE 1

/**
 * @brief  初始化SBUS解析器
 * @param  parser: SBUS解析器指针
 */
void SBUS_Init(SBUS_Parser *parser)
{
  if (parser == NULL)
    return;

  memset(parser, 0, sizeof(SBUS_Parser));
  parser->connected = FALSE;
  parser->last_update = 0;
}

/**
 * @brief  更新SBUS数据
 * @param  parser: SBUS解析器指针
 * @param  data: 原始数据指针
 * @param  len: 数据长度
 * @retval 解析是否成功
 */
uint8_t SBUS_Update(SBUS_Parser *parser, uint8_t *data, uint32_t len)
{
  if (parser == NULL || data == NULL || len < SBUS_FRAME_LENGTH)
  {
    return FALSE;
  }

  // 检查帧头和帧尾 如果帧头不是0x0F 或者 帧尾不是0x00，就返回 FALSE
  if (data[0] != 0x0F || data[SBUS_FRAME_LENGTH - 1] != 0x00)
  {
    return FALSE;
  }

  // 拷贝原始数据
  memcpy(&parser->raw_data, data, sizeof(SBUS_RawData));

  // 解析通道数据
  SBUS_ParseChannels(parser);

  parser->connected = TRUE;
  parser->last_update = HAL_GetTick();

  return TRUE;
}

/**
 * @brief  解析通道数据
 * @param  parser: SBUS解析器指针
 */
void SBUS_ParseChannels(SBUS_Parser *parser)
{
  if (parser == NULL)
    return;

  uint8_t *data = parser->raw_data.data;

  // 解析16个通道的11位数据
  parser->channels_raw[0] = ((data[0] | data[1] << 8) & 0x07FF);
  parser->channels_raw[1] = ((data[1] >> 3 | data[2] << 5) & 0x07FF);
  parser->channels_raw[2] = ((data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF);
  parser->channels_raw[3] = ((data[4] >> 1 | data[5] << 7) & 0x07FF);
  parser->channels_raw[4] = ((data[5] >> 4 | data[6] << 4) & 0x07FF);
  parser->channels_raw[5] = ((data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF);
  parser->channels_raw[6] = ((data[8] >> 2 | data[9] << 6) & 0x07FF);
  parser->channels_raw[7] = ((data[9] >> 5 | data[10] << 3) & 0x07FF);
  parser->channels_raw[8] = ((data[11] | data[12] << 8) & 0x07FF);
  parser->channels_raw[9] = ((data[12] >> 3 | data[13] << 5) & 0x07FF);
  parser->channels_raw[10] = ((data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF);
  parser->channels_raw[11] = ((data[15] >> 1 | data[16] << 7) & 0x07FF);
  parser->channels_raw[12] = ((data[16] >> 4 | data[17] << 4) & 0x07FF);
  parser->channels_raw[13] = ((data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF);
  parser->channels_raw[14] = ((data[19] >> 2 | data[20] << 6) & 0x07FF);
  parser->channels_raw[15] = ((data[20] >> 5 | data[21] << 3) & 0x07FF);

  /**
       通道值对应遥感映射关系
       channels_raw[0] 通道1: 右摇杆X 192~992~1792
       channels_raw[1] 通道2: 右摇杆Y 192~992~1792
       channels_raw[2] 通道3: 左摇杆Y 192~992~1792
       channels_raw[3] 通道4: 左摇杆X 192~992~1792
       channels_raw[4] 通道5: SWA-5  192/992/1792   三档自弹
       channels_raw[5] 通道6: SWB-6  192/1792       两档
       channels_raw[6] 通道7: SWC-7  192/1792       两档
       channels_raw[7] 通道8: SWD-8  192/992/1792   三档
       channels_raw[8] 通道9: VRA    192~1792       旋钮
       channels_raw[9] 通道10 VRB    192~1792       旋钮
       channels_raw[x] 通道11~16                    保留
  **/

  // 解析标志位
  uint8_t flags = parser->raw_data.flags;
  parser->ch17 = (flags & SBUS_FLAG_CH17) != 0;
  parser->ch18 = (flags & SBUS_FLAG_CH18) != 0;
  parser->frame_lost = (flags & SBUS_FLAG_LOST) != 0;
  parser->failsafe = (flags & SBUS_FLAG_FAILSAFE) != 0;

  // 转换成rbus 数据
  parser->rbus_parser.right_x = SBUS_To_RBUS_Range(parser->channels_raw[0]); // 通道1: 右摇杆X
  parser->rbus_parser.right_y = SBUS_To_RBUS_Range(parser->channels_raw[1]); // 通道2: 右摇杆Y
  parser->rbus_parser.left_y = SBUS_To_RBUS_Range(parser->channels_raw[2]);  // 通道3: 左摇杆Y
  parser->rbus_parser.left_x = SBUS_To_RBUS_Range(parser->channels_raw[3]);  // 通道4: 左摇杆X

  // 映射开关状态 (根据通道值判断三档开关)
  //  parser->rbus_parser->switch_mode = SBUS_Map_RBUS_Switch(parser->channels_raw[4]); // 使用通道5作为左1开关  保留
  parser->rbus_parser.switch_mode = SBUS_Map_RBUS_Switch(parser->channels_raw[5]);  // 使用通道6作为左2开关  模式
  parser->rbus_parser.switch_power = SBUS_Map_RBUS_Switch(parser->channels_raw[6]); // 使用通道7作为右1开关  开关
  parser->rbus_parser.switch_rate = SBUS_Map_RBUS_Switch(parser->channels_raw[7]);  // 使用通道8作为右2开关  速度
}

/**
 * @brief  将SBUS原始值转换RBUS标准范围
 * @param  sbus_value: SBUS原始值 (192-1792)
 * @retval RBUS标准范围值 (-100 ~ 100)
 */
int16_t SBUS_To_RBUS_Range(uint16_t sbus_value)
{
  int32_t result;

  // 限制输入范围不越界
  sbus_value = (sbus_value < SBUS_CH_MIN) ? SBUS_CH_MIN : (sbus_value > SBUS_CH_MAX) ? SBUS_CH_MAX
                                                                                     : sbus_value;

  // 线性映射计算
  // 公式: output = (input - mid) * range_ratio
  if (sbus_value > SBUS_CH_MID)
  {
    // 正值范围计算
    result = (int32_t)(sbus_value - SBUS_CH_MID) * RBUS_CH_MAX;
    result /= (SBUS_CH_MAX - SBUS_CH_MID);
  }
  else if (sbus_value < SBUS_CH_MID)
  {
    // 负值范围计算
    result = (int32_t)(sbus_value - SBUS_CH_MID) * RBUS_CH_MAX;
    result /= (SBUS_CH_MID - SBUS_CH_MIN);
  }
  else
  {
    // 中位值
    result = RBUS_CH_MID;
  }

  return (int16_t)result;
}

/**
 * @brief  映射开关状态
 * @param  channel_value: 通道原始值
 * @retval 开关状态 (SWITCH_UP/MID/DOWN)
 */
uint8_t SBUS_Map_RBUS_Switch(uint16_t channel_value)
{
  // 根据通道值映射三档开关 0是转换后的中间位置
  if (channel_value < SBUS_CH_MID)
    return RBUS_SWITCH_UP; // 下档
  else if (channel_value > SBUS_CH_MID)
    return RBUS_SWITCH_DOWN; // 上档
  else
    return RBUS_SWITCH_MID; // 中档
}

/**
 * @brief  检查连接超时
 * @param  parser: SBUS解析器指针
 * @retval 是否超时
 */
uint8_t SBUS_CheckTimeout(SBUS_Parser *parser)
{
  if (parser == NULL)
    return 1;

  uint32_t current_tick = HAL_GetTick();
  if (current_tick - parser->last_update > SBUS_TIMEOUT_MS)
  {
    parser->connected = FALSE;
    return 1;
  }

  return 0;
}

/**
 * @brief  打印通道数据（详细版）
 * @param  parser: SBUS解析器指针
 */
void SBUS_PrintChannels(SBUS_Parser *parser)
{
  if (parser == NULL || !parser->connected)
  {
    printf("SBUS: No data or disconnected\n");
    return;
  }

  printf("=== SBUS All Channels ===\n");
  // 打印原始值
  printf("Raw Values (0-2047):\n");
  for (int i = 0; i < 16; i++)
  {
    printf("CH%2d: %4d", i + 1, parser->channels_raw[i]);
    if ((i + 1) % 4 == 0)
      printf("\n");
    else
      printf(" | ");
  }

  printf("Digital: CH17=%d, CH18=%d\n", parser->ch17, parser->ch18);
  printf("Status: Lost=%d, Failsafe=%d\n", parser->frame_lost, parser->failsafe);
  printf("Connection: %s, Last Update: %lu ms\n",
         parser->connected ? "OK" : "Lost", parser->last_update);

  printf("=== RBUS Remote Data ===\n");
  printf("Left Stick:  X=%4d, Y=%4d\n", parser->rbus_parser.left_x, parser->rbus_parser.left_y);
  printf("Right Stick: X=%4d, Y=%4d\n", parser->rbus_parser.right_x, parser->rbus_parser.right_y);
  printf("Switches: sw-mode:%2d, sw-power:%2d, sw-rate=%4d\n", parser->rbus_parser.switch_mode,
         parser->rbus_parser.switch_power, parser->rbus_parser.switch_rate);
  printf("============END=============\n\n");
}

/**
 * @brief  获取通道原始值
 * @param  parser: SBUS解析器指针
 * @param  channel: 通道号 (0-15)
 * @retval 通道原始值 (0-2047)，如果错误返回0
 */
uint16_t SBUS_GetChannelRaw(SBUS_Parser *parser, uint8_t channel)
{
  if (parser == NULL || channel >= 16 || !parser->connected)
  {
    return 0;
  }

  return parser->channels_raw[channel];
}

/**
 * @brief  获取连接状态
 * @param  parser: SBUS解析器指针
 * @retval 连接状态
 */
uint8_t SBUS_IsConnected(SBUS_Parser *parser)
{
  if (parser == NULL)
    return FALSE;

  // 检查超时
  SBUS_CheckTimeout(parser);

  return parser->connected;
}

/**
 * @brief  获取帧丢失状态
 * @param  parser: SBUS解析器指针
 * @retval 帧丢失状态
 */
uint8_t SBUS_IsFrameLost(SBUS_Parser *parser)
{
  if (parser == NULL)
    return TRUE;

  return parser->frame_lost;
}

/**
 * @brief  获取故障安全状态
 * @param  parser: SBUS解析器指针
 * @retval 故障安全状态
 */
uint8_t SBUS_IsFailsafe(SBUS_Parser *parser)
{
  if (parser == NULL)
    return TRUE;

  return parser->failsafe;
}

/**
 * @brief  获取数字通道状态
 * @param  parser: SBUS解析器指针
 * @param  channel: 数字通道号 (17或18)
 * @retval 数字通道状态
 */
uint8_t SBUS_GetDigitalChannel(SBUS_Parser *parser, uint8_t channel)
{
  if (parser == NULL)
    return FALSE;

  if (channel == 17)
  {
    return parser->ch17;
  }
  else if (channel == 18)
  {
    return parser->ch18;
  }

  return FALSE;
}

/**
 * @brief  重置解析器状态
 * @param  parser: SBUS解析器指针
 */
void SBUS_Reset(SBUS_Parser *parser)
{
  if (parser == NULL)
    return;

  SBUS_Init(parser);
}

/******************* (C) COPYRIGHT 2024 SBUS Parser **************************/